
map = load('map.txt');
fname = sprintf('particle0.tx');
P = load(fname);

for i=1:1:1300
		
close all;
hold on;
axis "equal";
grid on;
%axis([-11 12 -10 14]);
axis([-4 2 -1 3]);
plotLineMap(map',0,'r');
dl = 0.2;
Px = dl * cos(P(:,3)) + P(:,1);
Py = dl * sin(P(:,3)) + P(:,2);

%Ppx = [P(:,1) Px];
%Ppy = [P(:,2) Py];

%PMap = [P Px Py]';

%plotLineMap(PMap,0,'g')

plot(P(:,1),P(:,2),'.');

picname = sprintf('%.4d_MCL_init.png',i-1);
print(picname,'-dpng')

%plot(Ppx,Ppy);


fname = sprintf('particle%d.tx',i);
P = load(fname);
%sleep(2)
end

